/*
 * cpp
 *
 *  Created on: 2020年9月2日
 *      Author: 17900
 */
#include "control/control_attitude.hpp"

/*
void EXTI15_10_IRQHandler(void)
{
	if(LL_EXTI_IsActiveFlag_0_31(LL_EXTI_LINE_10))
	{
		LL_EXTI_ClearFlag_0_31(LL_EXTI_LINE_10);
		height_msg height;
		xQueuePeek(queueHeight, &height, 0);					//从队列中获取高度数据
		static uint32_t MRS_PWM=0,MRS_PWM_Last=0;
		getTimer_us(&MRS_PWM);
		//height.MainRotorSpeed=1000000.0f/(MRS_PWM-MRS_PWM_Last);
		height.MainRotorSpeed=(float)MRS_PWM;
		MRS_PWM_Last=MRS_PWM;
	}
}
*/

void CONTROL_ALTITUDE::Integral_Init(){
	Pid_Reset(pidRolRate1);
	Pid_Reset(pidPitRate1);
	Pid_Reset(pidYawRate1);
	Pid_Reset(pidRolRate1_FF);
	Pid_Reset(pidPitRate1_FF);
	Pid_Reset(pidVelX);
	Pid_Reset(pidVelY);
	Pid_Reset(pidAcc_z);
}

void CONTROL_ALTITUDE::Control_Init()
{
	//积分清零
	Integral_Init();

	//roll_rate_pid
	pidRolRate1->Kp = 0.04f; //0.025f
	pidRolRate1->Ki = 0.08f;
	pidRolRate1->Kd = 0.0f;
	pidRolRate1->Kb = 0.0f;
	pidRolRate1->eLimit = 150.0f * D2R;
	pidRolRate1->iLimit = 90.0f * D2R;
	pidRolRate1->dLimit = PI;
	pidRolRate1->filter_para = 0.557f;

	//pitch_rate_pid
	pidPitRate1->Kp = 0.04f; //0.023f
	pidPitRate1->Ki = 0.08f;
	pidPitRate1->Kd = 0.0f;
	pidPitRate1->Kb = 0.0f;
	pidPitRate1->eLimit = 150.0f * D2R;
	pidPitRate1->iLimit = 90.0f * D2R;
	pidPitRate1->dLimit = PI;
	pidPitRate1->filter_para = 0.557f;

	//yaw_rate_pid
	pidYawRate1->Kp = 1.0f;
	pidYawRate1->Ki = 1.0f;
	pidYawRate1->Kd = 0.0f;
	pidYawRate1->Kb = 0.0f;
	pidYawRate1->eLimit = 4*PI;
	pidYawRate1->iLimit = PI;
	pidYawRate1->dLimit = PI;
	pidYawRate1->filter_para = 0.557f;

	//roll_angle_pid
	pidRol1->Kp = 5.0f; //5.0f;
	pidRol1->Ki = 0.00f;
	pidRol1->Kd = 0.0f;
	pidRol1->Kb = 0.0f;
	pidRol1->eLimit= 30.0f * D2R;
	pidRol1->iLimit = 0.5f*PI;
	pidRol1->filter_para = 0.557f;

	//pitch_angle pid
	pidPit1->Kp = 5.5f; //5.5f
	pidPit1->Ki = 0.00f;
	pidPit1->Kd = 0.0f;
	pidPit1->Kb = 0.0f;
	pidPit1->eLimit = 30.0f * D2R;
	pidPit1->iLimit = 0.5f*PI;
	pidPit1->filter_para = 0.557f;

	//yaw_angle pid
	pidYaw1->Kp = 4.0f;
	pidYaw1->Ki = 0.0f;
	pidYaw1->Kd = 0.0f;
	pidYaw1->Kb = 0.0f;
	pidYaw1->eLimit = PI;
	pidYaw1->iLimit = 0.5f*PI;
	pidYaw1->filter_para = 0.557f;

	//x方向速度pid
	pidVelX->Kp = 0.55f;
	pidVelX->Ki = 0.16f;
	pidVelX->Kd = 0.0f;
	pidVelX->Kb = 0.0f;
	pidVelX->eLimit = 4*PI;
	pidVelX->iLimit = PI;
	pidVelX->dLimit = PI;

	//y方向速度pid
	pidVelY->Kp = 0.54f;
	pidVelY->Ki = 0.16f;
	pidVelY->Kd = 0.0f;
	pidVelY->Kb = 0.0f;
	pidVelY->eLimit = 4*PI;
	pidVelY->iLimit = PI;
	pidVelY->dLimit = PI;

	//z方向速度pid
	pidVelZ->Kp = 3.0f;
	pidVelZ->Ki = 0.0f;
	pidVelZ->Kd = 0.0f;
	pidVelZ->Kb = 0.0f;
	pidVelZ->eLimit = 4*PI;
	pidVelZ->iLimit = PI;
	pidVelZ->dLimit = PI;

	//x位置
	pidX->Kp = 0.75f;
	pidX->Ki = 0.0f;
	pidX->Kd = 0.0f;
	pidX->Kb = 0.0f;
	pidX->iLimit = 20000.0f;

	//y位置
	pidY->Kp = 0.7f;
	pidY->Ki = 0.0f;
	pidY->Kd = 0.0f;
	pidY->Kb = 0.0f;
	pidY->iLimit = 20000.0f;

	//z位置
	pidZ->Kp = 2.2f;
	pidZ->Ki = 0.0f;
	pidZ->Kd = 0.0f;
	pidZ->Kb = 0.0f;
	pidZ->iLimit = 20000.0f;

	//高度(z方向)加速度
	pidAcc_z->Kp = 0.2f;
	pidAcc_z->Ki = 0.2f;

	//角速度前馈值
	pidRolRate1_FF->Kp = 0.06f; //从角速度过来的前馈值
	pidRolRate1_FF->Ki = 0.0f;
	pidRolRate1_FF->Kd = 0.0f;
	pidRolRate1_FF->Kb = 0.0f;
	pidRolRate1_FF->iLimit = 20000.0f;

	pidPitRate1_FF->Kp = 0.05f;
	pidPitRate1_FF->Ki = 0.0f;
	pidPitRate1_FF->Kd = 0.0f;
	pidPitRate1_FF->Kb = 0.0f;
	pidPitRate1_FF->iLimit = 20000.0f;

	pwm_coll_mid = 1500;
	pwm_pitch_mid = 1500;
	pwm_roll_mid = 1500;
	pwm_yaw_mid = 1500;
	auto_roll_control_mode=1;
	auto_pitch_control_mode=1;
	auto_yaw_control_mode=1;
	auto_coll_control_mode=1;
	rate_error = 0.0f;
	_rate_error = 0.0f;
	target_rate = 0.0f;
	yaw_out = 0.0f;
	raw_target_rate = 0.0f;
	flag_breach_limit = false;
	clear_flag = false;
	parameter_flag = false;
	Pos_Loop_count = 0;
	Att_Loop_count = 0;
	yaw_rate_command_from_outloop = 0.0f;

	for(u8 i=0;i<PID_NUM;i++)
	{
		pid_msg.kp[i] = pid[i].Kp;
		pid_msg.ki[i] = pid[i].Ki;
		pid_msg.kd[i] = pid[i].Kd;
	}
	xQueueOverwrite(queuePID,&pid_msg);
}

void CONTROL_ALTITUDE::Angle_From_Rc_Or_Outloop(void)
{
	if(control_mode == MODE_ALTITUDE)
	{
		roll_command_from_rc = ((abs((ROLL_PWM_REMOTE_MID-manual_servo_width[0])))>20.0f?(ROLL_PWM_REMOTE_MID-manual_servo_width[0]):0.0f)/8.0f*D2R + (trim_roll_angle * D2R);

		pitch_command_from_rc = ((abs((PITCH_PWM_REMOTE_MID-manual_servo_width[1])))>20.0f?(manual_servo_width[1]- PITCH_PWM_REMOTE_MID):0.0f)/12.0f*D2R + (trim_pitch_angle * D2R);
//		roll_command_from_rc = ((abs((ROLL_PWM_REMOTE_MID-manual_servo_width[0])))>20.0f?(ROLL_PWM_REMOTE_MID-manual_servo_width[0]):0.0f)/8.0f*D2R + (pidTest3->Kp * D2R);
//
//		pitch_command_from_rc = ((abs((PITCH_PWM_REMOTE_MID-manual_servo_width[1])))>20.0f?(manual_servo_width[1]- PITCH_PWM_REMOTE_MID):0.0f)/12.0f*D2R + (pidTest3->Ki * D2R);

		yaw_rate_command_from_rc = (((abs((YAW_PWM_REMOTE_MID-manual_servo_width[3]))))>70.0f?(YAW_PWM_REMOTE_MID-manual_servo_width[3]):0.0f)/4.0f*D2R;
	}
	else if(control_mode == MODE_POSITION_HOLD)
	{
		roll_command_from_rc = roll_command_from_outloop + (trim_roll_angle * D2R);

		pitch_command_from_rc = pitch_command_from_outloop + (trim_pitch_angle * D2R);

		yaw_rate_command_from_rc = yaw_rate_command_from_outloop;

	}
}

void CONTROL_ALTITUDE::Ang_Loop_Feedback(void)
{
	xQueuePeek(queueESKF, &eskf_output, 0);

	roll_feedback = eskf_output.Attitude[0];
	pitch_feedback = eskf_output.Attitude[1];
	yaw_feedback = eskf_output.Attitude[2];
}

void CONTROL_ALTITUDE::Ang_Rate_Loop_Feedback(void)
{
	xQueuePeek(queueGyrDatFil, &gyro_filter, 0);
	xQueuePeek(queueESKF, &eskf_output, 0);

//	roll_rate_feedback = gyro_filter.gyro_filter[0] - eskf_output.Gyro_bias[0];
//	pitch_rate_feedback = gyro_filter.gyro_filter[1] - eskf_output.Gyro_bias[1];
	roll_rate_feedback = gyro_filter.gyro_filter[0];
	pitch_rate_feedback = gyro_filter.gyro_filter[1];
}

float CONTROL_ALTITUDE::sqrt_controller(float error, float p, float second_ord_lim)
{
    float linear_dist = second_ord_lim/(p*p);

    if (error > linear_dist) {
        return sqrtf(2.0f*second_ord_lim*(error-(linear_dist/2.0f)));
    } else if (error < -linear_dist) {
        return -sqrtf(2.0f*second_ord_lim*(-error-(linear_dist/2.0f)));
    } else {
        return error*p;
    }
}

void CONTROL_ALTITUDE::update_ef_roll_pitch_angle_and_error(void)
{
	angle_error_ef[0] = fConstrain(roll_command - roll_feedback,-50.0f * D2R, 50.0f * D2R);
	angle_error_ef[1] = fConstrain(pitch_command - pitch_feedback,-50.0f * D2R, 50.0f * D2R);
	angle_error_ef[2] = yaw_command - yaw_feedback;

	if(angle_error_ef[2] > 180 * D2R)
	{
		angle_error_ef[2] = angle_error_ef[2] - 2 * 180 * D2R;
	}
	else if (angle_error_ef[2] < -180 * D2R)
	{
		angle_error_ef[2] = angle_error_ef[2] + 2 * 180 * D2R;
	}

	roll_command += roll_rate_feedforward * CONTROL_DT;
	pitch_command += pitch_rate_feedforward * CONTROL_DT;
	yaw_command += yaw_rate_feedforward * CONTROL_DT;

	if(yaw_command  > 180 * D2R)
	{
		yaw_command  = yaw_command - 2 * 180 * D2R;
	}
	else if (yaw_command  < -180 * D2R)
	{
		yaw_command  = yaw_command  + 2 * 180 * D2R;
	}
}

void CONTROL_ALTITUDE::Angle_error_ef_2_bf(void)
{
	xQueuePeek(queueESKF, &eskf_output, 0);

	angle_error_bf[0] = angle_error_ef[0] - sinf(eskf_output.Attitude[1]) * angle_error_ef[2];
	angle_error_bf[1] = cosf(eskf_output.Attitude[0])  * angle_error_ef[1] + sinf(eskf_output.Attitude[0]) * cosf(eskf_output.Attitude[1]) * angle_error_ef[2];
	angle_error_bf[2] = -sinf(eskf_output.Attitude[0]) * angle_error_ef[1] + cosf(eskf_output.Attitude[1]) * cosf(eskf_output.Attitude[0]) * angle_error_ef[2];
}

void CONTROL_ALTITUDE::update_rate_bf_targets(void)
{
	 roll_rate_command = sqrt_controller(angle_error_bf[0], pidRol1->Kp, fConstrain(ACCEL_ROLL_MAX/2.0f * D2R,  AC_ATTITUDE_ACCEL_RP_CONTROLLER_MIN * D2R, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MAX * D2R)) + roll_rate_feedforward_bf;
	 pitch_rate_command = sqrt_controller(angle_error_bf[1], pidPit1->Kp, fConstrain(ACCEL_PITCH_MAX/2.0f * D2R,  AC_ATTITUDE_ACCEL_RP_CONTROLLER_MIN * D2R, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MAX * D2R)) + pitch_rate_feedforward_bf;
	 yaw_rate_command = sqrt_controller(angle_error_bf[2], pidYaw1->Kp, fConstrain(ACCEL_YAW_MAX/2.0f * D2R,  AC_ATTITUDE_ACCEL_Y_CONTROLLER_MIN * D2R, AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX * D2R)) +  yaw_rate_feedforward_bf;
}

void CONTROL_ALTITUDE::Angle_Loop(void)
{
	float rate_ef_desired;
	float rate_change_limit;

	xQueuePeek(queueESKF, &eskf_output, 0);

	//算出前馈roll角速度的值
	rate_change_limit = ACCEL_ROLL_MAX * CONTROL_DT * D2R;//角速度限制，在一个采样周期中（10ms），角速度最大变化 = 最大角加速度 * 周期 * D2R
	rate_ef_desired = sqrt_controller(roll_command_from_rc - roll_command, SMOOTHING_GAIN, ACCEL_ROLL_MAX * D2R);//根据当前遥控器给定目标值与当前控制器的目标值的差，计算角速度前馈
	roll_rate_feedforward = fConstrain(rate_ef_desired, roll_rate_feedforward - rate_change_limit, roll_rate_feedforward+rate_change_limit);//计算出的角速度前馈限幅

	//算出前馈pitch角速度的值
	rate_change_limit = ACCEL_PITCH_MAX * CONTROL_DT * D2R;
	rate_ef_desired = sqrt_controller(pitch_command_from_rc -pitch_command, SMOOTHING_GAIN, ACCEL_PITCH_MAX * D2R);
	pitch_rate_feedforward = fConstrain(rate_ef_desired, pitch_rate_feedforward-rate_change_limit, pitch_rate_feedforward+rate_change_limit);

	//算出前馈yaw角速度的值，前馈角速度值逐渐向RC给定期望角速度靠近
	rate_change_limit = ACCEL_YAW_MAX * CONTROL_DT * D2R;
	yaw_rate_feedforward += fConstrain(yaw_rate_command_from_rc - yaw_rate_feedforward,-rate_change_limit,rate_change_limit);

	//机体角速度body frame与欧拉角速度earth frame之间的转换
	roll_rate_feedforward_bf = roll_rate_feedforward - sinf(eskf_output.Attitude[1]) * yaw_rate_feedforward;
	pitch_rate_feedforward_bf = cosf(eskf_output.Attitude[0])  * pitch_rate_feedforward + sinf(eskf_output.Attitude[0]) * cosf(eskf_output.Attitude[1]) * yaw_rate_feedforward;
	yaw_rate_feedforward_bf = -sinf(eskf_output.Attitude[0]) * pitch_rate_feedforward + cosf(eskf_output.Attitude[1]) * cosf(eskf_output.Attitude[0]) * yaw_rate_feedforward;

	update_ef_roll_pitch_angle_and_error(); //该函数计算出roll、pitch、yaw三个角度在一个周期0.01s的变化值，更新command值
	Angle_error_ef_2_bf();
	update_rate_bf_targets();//更新角速度command值
}

void CONTROL_ALTITUDE::Angle_Rate_Loop(void)
{
	 #define roll_rate_alpha 0.55f  //20Hz
	 #define pitch_rate_alpha 0.55f //20Hz
	 #define roll_rate_feedfoward_alpha 0.385f//10Hz
	 #define pitch_rate_feedfoward_alpha 0.385f//10Hz

	 double angle_rate_error;
	 double d_error;
	 double rate_FF;
	 double d_rate_FF;

	 //滚转通道
	 rate_FF = roll_rate_command * pidRolRate1_FF->Kp;//计算前馈量
	 d_rate_FF = (rate_FF - pidRolRate1_FF->lastError) * roll_rate_feedfoward_alpha;//计算低通增量
	 pidRolRate1_FF->error = pidRolRate1_FF->lastError + d_rate_FF;
	 pidRolRate1_FF->lastError = pidRolRate1_FF->error;

	 angle_rate_error = roll_rate_command - roll_rate_feedback;//滚转角速度误差
	 d_error = (angle_rate_error - pidRolRate1->lastError) * roll_rate_alpha;//低通滤波器解算出误差量
	 pidRolRate1->error = pidRolRate1->lastError + d_error;//在原来的基础上叠加误差量
	 pidRolRate1->error = fConstrain(pidRolRate1->error,-157.3f * D2R, 157.3f * D2R);//限幅
	 pidRolRate1->lastError = pidRolRate1->error;

	 pidRolRate1->integral = fConstrain(pidRolRate1->integral + pidRolRate1->error * CONTROL_DT,-90.0f * D2R, 90.0f * D2R);
	 pidRolRate1->pout = pidRolRate1->error * pidRolRate1->Kp;
	 pidRolRate1->iout = pidRolRate1->integral * pidRolRate1->Ki;

	 output[0]= fConstrain(pidRolRate1->pout + pidRolRate1->iout + pidRolRate1_FF->error,-57.3f * D2R, 57.3f * D2R) ;
	 if(auto_roll_control_mode == 1)
	 {
		 auto_servo_width[0] = pwm_roll_mid - output[0] * RAD2PWM;
	 }
	 //俯仰通道
	 rate_FF = pitch_rate_command * pidPitRate1_FF->Kp;//计算前馈量
	 d_rate_FF = (rate_FF - pidPitRate1_FF->lastError) * roll_rate_feedfoward_alpha;//计算低通增量量
	 pidPitRate1_FF->error = pidPitRate1_FF->lastError + d_rate_FF;
	 pidPitRate1_FF->lastError = pidPitRate1_FF->error;

	 angle_rate_error = pitch_rate_command - pitch_rate_feedback;//俯仰角速度误差
	 d_error = (angle_rate_error - pidPitRate1->lastError) * pitch_rate_alpha;//低通滤波器解算出误差量
	 pidPitRate1->error = pidPitRate1->lastError + d_error;//在原来的基础上叠加误差量
	 pidPitRate1->error = fConstrain(pidPitRate1->error,-157.3f * D2R, 157.3f * D2R);//限幅
	 pidPitRate1->lastError =  pidPitRate1->error;

	 pidPitRate1->integral = fConstrain(pidPitRate1->integral + pidPitRate1->error * CONTROL_DT,-90.0f * D2R, 90.0f * D2R);
	 pidPitRate1->iout = pidPitRate1->integral * pidPitRate1->Ki;
	 pidPitRate1->pout = pidPitRate1->error * pidPitRate1->Kp;

	 output[1]= fConstrain(pidPitRate1->pout + pidPitRate1->iout + pidPitRate1_FF->error,-57.3f * D2R, 57.3f * D2R) ;
	 if(auto_pitch_control_mode == 1)
	 {
		 auto_servo_width[1] = pwm_pitch_mid + output[1] * RAD2PWM;
	 }
	 //航向通道
	 if(auto_yaw_control_mode == 1)
	 {
		 auto_servo_width[3] = pwm_yaw_mid + yaw_rate_command/GAIN_REMOTE_TO_RATE;
	 }
}


void CONTROL_ALTITUDE::Attitude_Control_Step(void)
{
	Angle_From_Rc_Or_Outloop();

	Ang_Loop_Feedback();

	Ang_Rate_Loop_Feedback();

	Angle_Loop();

	Angle_Rate_Loop();

	if(auto_servo_width[7] < 1500){//开关向下，正常锁尾模式
		Avcs();
//		Avcs2();
//		clear_flag = true;
	}
	else{
		ESO_Avcs1();
		//ESO_Avcs2();
	}
}
void CONTROL_ALTITUDE::Flybarless_H1_DecoupingMatrix(void)
{
	xQueuePeek(queueRCCommand, &rcCommand, 0);
	//控制内环的期望值输入manual_servo_width
	//控制内环的输出值auto_servo_width
	manual_servo_width[0] = rcCommand.PPM[0]; //manual_servo_width[0]是roll通道
	manual_servo_width[1] = rcCommand.PPM[1]; //manual_servo_width[1]是pitch通道
	manual_servo_width[2] = rcCommand.PPM[5]; //manual_servo_width[3]是coll通道
	manual_servo_width[3] = rcCommand.PPM[3]; //manual_servo_width[3]是yaw_rate通道

	auto_servo_width[0] = rcCommand.PPM[0];
	auto_servo_width[1] = rcCommand.PPM[1];
	auto_servo_width[2] = rcCommand.PPM[2];
	auto_servo_width[3] = rcCommand.PPM[3];
	auto_servo_width[4] = rcCommand.PPM[4];
	auto_servo_width[5] = rcCommand.PPM[5];
	auto_servo_width[6] = rcCommand.PPM[6];
	auto_servo_width[7] = rcCommand.PPM[7];

}

void CONTROL_ALTITUDE::Flybarless_CoupingMatrix(void)
{
	auto_servo_width_temp[0] = (auto_servo_width[0]-1500) ;//roll 1488
	auto_servo_width_temp[1] = (auto_servo_width[1]-1500) ;//pitch 1538
	auto_servo_width_temp[2] = (auto_servo_width[5]-1500) * 3/5;//coll，总距不走控制通道，直接由遥控器控制

	//左、后、右舵机
	couple_servo_width[0] =  auto_servo_width_temp[0] * sqrtf(3) /2 - auto_servo_width_temp[1]/2 + auto_servo_width_temp[2] + 1500;
	couple_servo_width[1] =  									    + auto_servo_width_temp[1]   + auto_servo_width_temp[2] + 1500;
	couple_servo_width[2] =  auto_servo_width_temp[0] * sqrtf(3) /2 + auto_servo_width_temp[1]/2 - auto_servo_width_temp[2] + 1500;
}


void CONTROL_ALTITUDE::Flybarless_servos_widths_output(void)
{
	xQueuePeek(queueControlOutputDat, &control_output, 0);
	control_output.cs_output[0] = couple_servo_width[0];
	control_output.cs_output[1] = couple_servo_width[1];
	control_output.cs_output[2] = couple_servo_width[2];
	control_output.cs_output[3] = auto_servo_width[3];
	control_output.mt_output[0] = rcCommand.PPM[2]; //油门
	control_output.control_mode = control_mode;
	xQueueOverwrite(queueControlOutputDat,&control_output);
}

float test_kp,test_ki,test_target_rate;
void CONTROL_ALTITUDE::Avcs(void)
{
	float I_increment = 0.0f;
	float rate_error_for_I = 0.0f;
	float d_rate_error = 0.0f;
	int coll_PWM_inc = 0;
	float coll_yaw_compensation_gain = 0.0f;
	int yaw_offset = 0.0f;
	int target_pwm = 0;

	xQueuePeek(queueGyrDatFil, &gyro_filter, 0);
	xQueuePeek(queueESKF, &eskf_output, 0);
	xQueuePeek(queueADRC, &adrc, 0);

	target_pwm = auto_servo_width[3] - pwm_yaw_mid;

	if((control_mode == MODE_MANUAL) || (control_mode == MODE_ALTITUDE))
	{
		if(target_pwm > 30)//23
			target_pwm = target_pwm - 30;
		else if(target_pwm < -30)
			target_pwm = target_pwm + 30;
		else target_pwm = 0;
	}

	raw_target_rate = target_pwm * GAIN_REMOTE_TO_RATE;
	if(fabs(raw_target_rate)>=fabs(target_rate))
	{
		float e_tr = raw_target_rate  - target_rate;
		if(e_tr > RATE_DECREASEMENT)
			target_rate += RATE_DECREASEMENT;
		else if(e_tr < -RATE_DECREASEMENT)
			target_rate -= RATE_DECREASEMENT;
		else
			target_rate += e_tr;
	}
	else
	{
		float e_tr = raw_target_rate  - target_rate;
		if (e_tr > RATE_INCREASEMENT)
			target_rate += RATE_INCREASEMENT;
		else if(e_tr < -RATE_INCREASEMENT)
			target_rate -= RATE_INCREASEMENT;
		else
			target_rate += e_tr;
	}
	if (target_rate > LIMIT_RATE)
		target_rate = LIMIT_RATE;
	if (target_rate < -LIMIT_RATE)
		target_rate = -LIMIT_RATE;
	test_target_rate = target_rate;

	rate_error = (target_rate - gyro_filter.gyro_filter[2]);
	rate_error_for_I = (target_rate - gyro_filter.gyro_filter[2]) * CONTROL_DT;

	//低通滤波，没用上
	d_rate_error = (rate_error - _rate_error)*ALPHA;
	rate_error = d_rate_error + _rate_error;
	_rate_error = rate_error;

	coll_PWM_inc = pwm_coll_mid - auto_servo_width[5]; //总距PWM增加值，这是前馈通道补偿值，利用前馈通道使得尾翼快速跟踪总距的变化
	coll_yaw_compensation_gain = 0.1 * 0.00864;//out_loop_Y_PID[0] 0.42f pidVelX->Kp
	yaw_offset = 0.0f;
//	yaw_offset = coll_yaw_compensation_gain * coll_PWM_inc; //增益通过实验测出

/******************************拨动开关，选择控制器为P*************************************/
	if((auto_servo_width[4]) <= 1500)
	{
		#ifdef _AVCS_TUNING_ //用于转盘调节锁尾PI
			pidYawRate1->Kp = 0.3240f * (1500 - ele->auto_servo_width[4]);

		#endif
		pidYawRate1->integral = 0.0; //清除积分
		flag_breach_limit = 0; //抗集分饱和标志位

		yaw_out = rate_error * pidYawRate1->Kp+ yaw_offset;   //角速度输出值
		test_kp = pidYawRate1->Kp;
		//控制量限幅
		if(yaw_out > OUTPUT_LIMIT)
		{
			yaw_out = OUTPUT_LIMIT;
		}
		else if(yaw_out < -OUTPUT_LIMIT)
		{
			yaw_out = -OUTPUT_LIMIT;
		}
		auto_servo_width[3] = pwm_yaw_mid + yaw_out / GAIN_REMOTE_TO_RATE; // pwm_yaw_mid - yaw_out
	}
	else
	{
		#ifdef _AVCS_TUNING_
		pidYawRate1->Ki = 0.3240f * (ele->auto_servo_width[4] - 1500);
		#endif
/************************拨动开关，选择控制器为PI*****************************************/
		I_increment = pidYawRate1->Ki * rate_error_for_I;
		//抗积分饱和
		if(((!flag_breach_limit) || (pidYawRate1->integral>0&&I_increment<0) || (pidYawRate1->integral<0&&I_increment>0)))
		{
			pidYawRate1->integral += I_increment; //如果积分未饱和，那么I(k) = I(k - 1) + ΔI，至于为什么I_increment需要乘采样周期T，因为增量的积分符号离散化就是e(k)*T
		}
		//角速度输出值 = K通道角速度输出值 + I通道角速度输出值
		yaw_out = pidYawRate1->Kp * rate_error + pidYawRate1->integral+ yaw_offset;// + coll_rate *in_loop_yaw_PID[1] + gain_D*d_rate_error;//*2.0*(1.0+abs(rate_error/6.0))
		//test_kp = pidYawRate1->Kp * rate_error;
		test_kp = pidYawRate1->Kp;
		//test_ki = pidYawRate1->integral;
		test_ki = pidYawRate1->Ki;
		//控制量限幅
		if(yaw_out > OUTPUT_LIMIT)
		{
			flag_breach_limit = 1;
			yaw_out = OUTPUT_LIMIT;
		}
		else if(yaw_out < -OUTPUT_LIMIT)
		{
			flag_breach_limit = 1;
			yaw_out = -OUTPUT_LIMIT;
		}
		else
		{
			flag_breach_limit = 0;
		}
		auto_servo_width[3] = pwm_yaw_mid + yaw_out / GAIN_REMOTE_TO_RATE; // pwm_yaw_mid - yaw_out
	}
	adrc.yaw_out = yaw_out;
	adrc.target_rate = target_rate;
	xQueueOverwrite(queueADRC, &adrc);
}

/*
 * 基于ESO + PI的抗扰控制器
 * */
void CONTROL_ALTITUDE::ESO_Avcs1(){
	float I_increment = 0.0f;
	float rate_error_for_I = 0.0f;
	int coll_PWM_inc = 0;
	float coll_yaw_compensation_gain = 0.0f;
	int yaw_offset = 0.0f;
	int target_pwm = 0;

	float e = 0.0f;
	float fe = 0.0f;
	float fe1 = 0.0f;
	float h = CONTROL_DT;
	static float ESO_state[3] = {0, 0, 0};
	if(clear_flag == true){
		for(int idx = 0; idx < 3; ++idx){
			ESO_state[idx] = 0.0f;
		}
		yaw_out = 0.0f;
		clear_flag = false;
	}
	xQueuePeek(queueGyrDatFil, &gyro_filter, 0);
	xQueuePeek(queueESKF, &eskf_output, 0);
	xQueuePeek(queueADRC, &adrc, 0);

	target_pwm = auto_servo_width[3] - pwm_yaw_mid;

	if((control_mode == MODE_MANUAL) || (control_mode == MODE_ALTITUDE))
	{
		if(target_pwm > 30)//23
			target_pwm = target_pwm - 30;
		else if(target_pwm < -30)
			target_pwm = target_pwm + 30;
		else target_pwm = 0;
	}

	raw_target_rate = target_pwm * GAIN_REMOTE_TO_RATE;
	if(fabs(raw_target_rate)>=fabs(target_rate))
	{
		float e_tr = raw_target_rate  - target_rate;
		if(e_tr > RATE_DECREASEMENT)
			target_rate += RATE_DECREASEMENT;
		else if(e_tr < -RATE_DECREASEMENT)
			target_rate -= RATE_DECREASEMENT;
		else
			target_rate += e_tr;
	}
	else
	{
		float e_tr = raw_target_rate  - target_rate;
		if (e_tr > RATE_INCREASEMENT)
			target_rate += RATE_INCREASEMENT;
		else if(e_tr < -RATE_INCREASEMENT)
			target_rate -= RATE_INCREASEMENT;
		else
			target_rate += e_tr;
	}
	if (target_rate > LIMIT_RATE)
		target_rate = LIMIT_RATE;
	if (target_rate < -LIMIT_RATE)
		target_rate = -LIMIT_RATE;
	test_target_rate = target_rate;

	rate_error = (target_rate - gyro_filter.gyro_filter[2]);
	rate_error_for_I = (target_rate - gyro_filter.gyro_filter[2]) * CONTROL_DT;

	coll_PWM_inc = pwm_coll_mid - auto_servo_width[5]; //总距PWM增加值，这是前馈通道补偿值，利用前馈通道使得尾翼快速跟踪总距的变化
	coll_yaw_compensation_gain = 0.1 * 0.00864;//out_loop_Y_PID[0] 0.42f pidVelX->Kp
	yaw_offset = 0.0f;
//	yaw_offset = coll_yaw_compensation_gain * coll_PWM_inc; //增益通过实验测出

	I_increment = pidYawRate1->Ki * rate_error_for_I;
	pidYawRate1->integral += I_increment; //如果积分未饱和，那么I(k) = I(k - 1) + ΔI，至于为什么I_increment需要乘采样周期T，因为增量的积分符号离散化就是e(k)*T

	/*
	 * TD部分：未用到
	 * @args
	 * r:速度因子，当速度因子越小，v1曲线越平缓；速度因子越大，v1曲线变化越大，并无限接近于v
	 * h0：一般比h大，当h0较大时，能够明显减少震荡，但是太大又会造成v1的变化过于平缓
	 * */
	static float TD_state[2] = {0, 0};
	static float r = 10.0f; //50.0f
	static float h0 = h; // 40 * h

	if(pidAcc_z->Kp != 0 && pidAcc_z->Ki != 0){
		r = pidAcc_z->Kp * 1000;
		h0 = pidAcc_z->Ki * 1000 * h;
	}else{
		r = 10.0f;
		h0 = h;
	}
	float fh = fhan(TD_state[0] - target_rate,TD_state[1], r, h0);
	TD_state[0] = TD_state[0] + h * TD_state[1];
	TD_state[1] = TD_state[1] + h * fh;



	//ESO部分
	static float beta01 = 100.0f; //100x
	static float beta02 = 540.0f; //300
	static float beta03 = 1000.0f; //1000
	static float delta = h; // h
	static float b0 = 100.0f; //补偿因子，过小会使u产生震荡，过大会抑制v1的     //过大！

	if(parameter_flag == true){ //调参模式
		beta01 = pidTest1->Kp * 1000, beta02 = pidTest1->Ki * 1000, beta03 = pidTest1->Kd * 1000;
		b0 = (pidTest2->Kp == 0.0f) ? 1 : pidTest2->Kp * 1000;
	}
	e = ESO_state[0] - (gyro_filter.gyro_filter[2]);
	fe = fal(e, 0.5, delta);
	fe1 = fal(e, 0.25, delta);
	ESO_state[0] = ESO_state[0] + h * (ESO_state[1] - beta01 * e);
	ESO_state[1] = ESO_state[1] + h * (ESO_state[2] - beta02 * fe + b0 * yaw_out);
	ESO_state[2] = ESO_state[2] + h * (-beta03 * fe1);

	//角速度输出值 = K通道角速度输出值 + I通道角速度输出值
	yaw_out = pidYawRate1->Kp * rate_error + pidYawRate1->integral - ESO_state[2] / b0;
	//控制量限幅
	if(yaw_out > OUTPUT_LIMIT)
	{
		yaw_out = OUTPUT_LIMIT;
	}
	else if(yaw_out < -OUTPUT_LIMIT)
	{
		yaw_out = -OUTPUT_LIMIT;
	}

	auto_servo_width[3] = pwm_yaw_mid + yaw_out / GAIN_REMOTE_TO_RATE; // pwm_yaw_mid - yaw_out

	for(int idx = 0; idx < 3; ++idx){ // z1 z2 z3
		adrc.ESO_state[idx] = ESO_state[idx];
	}
	for(int idx = 0; idx < 2; ++idx){  // v1 v2
		adrc.TD_state[idx] = TD_state[idx];
	}
	adrc.yaw_out = yaw_out; // u
	adrc.target_rate = target_rate; //v
	xQueueOverwrite(queueADRC, &adrc);
}

/*
 * 基于ESO + TD + PD的抗扰控制器，详见《小型无人直升机飞行控制与运动规划》第三章
 * */
void CONTROL_ALTITUDE::ESO_Avcs2(){
	static float TD_state[2] = {0, 0};
	static float ESO_state[3] = {0, 0, 0};
	static float e1 = 0.0f;
	static float e2 = 0.0f;

//	float I_increment = 0.0f;
//	float rate_error_for_I = 0.0f;
//	int coll_PWM_inc = 0;
//	float coll_yaw_compensation_gain = 0.0f;
//	int yaw_offset = 0.0f;
	int target_pwm = 0;

	float e = 0.0f;
	float fe = 0.0f;
	float fe1 = 0.0f;
	float h = CONTROL_DT;
	float fh = 0.0f;
	if(clear_flag == true){
		for(int idx = 0; idx < 2; ++idx){
			TD_state[idx] = 0.0f;
			ESO_state[idx] = 0.0f;
		}
		ESO_state[2] = 0.0f;
		yaw_out = 0.0f;
		clear_flag = false;
	}
	xQueuePeek(queueGyrDatFil, &gyro_filter, 0);
	xQueuePeek(queueESKF, &eskf_output, 0);
	xQueuePeek(queueADRC, &adrc, 0);

	target_pwm = auto_servo_width[3] - pwm_yaw_mid;

	if((control_mode == MODE_MANUAL) || (control_mode == MODE_ALTITUDE))
	{
		if(target_pwm > 30)//23
			target_pwm = target_pwm - 30;
		else if(target_pwm < -30)
			target_pwm = target_pwm + 30;
		else target_pwm = 0;
	}

	raw_target_rate = target_pwm * GAIN_REMOTE_TO_RATE;
	if(fabs(raw_target_rate)>=fabs(target_rate))
	{
		float e_tr = raw_target_rate  - target_rate;
		if(e_tr > RATE_DECREASEMENT)
			target_rate += RATE_DECREASEMENT;
		else if(e_tr < -RATE_DECREASEMENT)
			target_rate -= RATE_DECREASEMENT;
		else
			target_rate += e_tr;
	}
	else
	{
		float e_tr = raw_target_rate  - target_rate;
		if (e_tr > RATE_INCREASEMENT)
			target_rate += RATE_INCREASEMENT;
		else if(e_tr < -RATE_INCREASEMENT)
			target_rate -= RATE_INCREASEMENT;
		else
			target_rate += e_tr;
	}
	if (target_rate > LIMIT_RATE)
		target_rate = LIMIT_RATE;
	if (target_rate < -LIMIT_RATE)
		target_rate = -LIMIT_RATE;
	test_target_rate = target_rate;

	/*
	 * TD部分
	 * @args
	 * r:速度因子，当速度因子越小，v1曲线越平缓；速度因子越大，v1曲线变化越大，并无限接近于v
	 * h0：一般比h大，当h0较大时，能够明显减少震荡，但是太大又会造成v1的变化过于平缓
	 * */
	//args
	static float r = 10.0f; //50.0f
	static float h0 = h; // 40 * h

	if(pidAcc_z->Kp != 0 && pidAcc_z->Ki != 0){
		r = pidAcc_z->Kp * 1000;
		h0 = pidAcc_z->Ki * 1000 * h;
	}else{
		r = 10.0f;
		h0 = h;
	}
	fh = fhan(TD_state[0] - target_rate,TD_state[1], r, h0);
	TD_state[0] = TD_state[0] + h * TD_state[1];
	TD_state[1] = TD_state[1] + h * fh;


	//ESO部分
	static float beta01 = 100.0f; //100x
	static float beta02 = 200.0f; //300
	static float beta03 = 500.0f; //1000
	static float delta = h; // h
	static float b0 = 100.0f; //补偿因子，过小会使u产生震荡，过大会抑制v1的     //过大！

	beta01 = pidTest1->Kp * 1000, beta02 = pidTest1->Ki * 1000, beta03 = pidTest1->Kd * 1000;
	if(pidTest2->Kd != 0.0f){
		b0 = pidTest2->Kd * 1000;
	}else{
		b0 = 1.0f;
	}
	e = ESO_state[0] - (gyro_filter.gyro_filter[2]);
	fe = fal(e, 0.5, delta);
	fe1 = fal(e, 0.25, delta);
	ESO_state[0] = ESO_state[0] + h * (ESO_state[1] - beta01 * e);
	ESO_state[1] = ESO_state[1] + h * (ESO_state[2] - beta02 * fe + b0 * yaw_out);
	ESO_state[2] = ESO_state[2] + h * (-beta03 * fe1);

	e1 = TD_state[0] - ESO_state[0];
	e2 = TD_state[1] - ESO_state[1];
	//角速度输出值 = K通道角速度输出值 + I通道角速度输出值
	static float testK = 0.0f;
	if(pidAcc_z->Kd != 0.0f){
		testK = pidAcc_z->Kd * 1000;
	}else{
		testK = 1.0f;
	}
	//yaw_out = pidTest2->Kp * 100 * e1 + pidTest2->Ki * 100 * e2 - ESO_state[2] / (b0 * testK); //Kp = 100, Ki = 100
	yaw_out = pidTest2->Kp * 100 * e1 + pidTest2->Ki * 100 * e2;
	//控制量限幅
	if(yaw_out > OUTPUT_LIMIT)
	{
		yaw_out = OUTPUT_LIMIT;
	}
	else if(yaw_out < -OUTPUT_LIMIT)
	{
		yaw_out = -OUTPUT_LIMIT;
	}
	auto_servo_width[3] = pwm_yaw_mid + yaw_out / (GAIN_REMOTE_TO_RATE); // pwm_yaw_mid - yaw_out

	for(int idx = 0; idx < 2; ++idx){
		adrc.TD_state[idx] = TD_state[idx];
		adrc.ESO_state[idx] = ESO_state[idx];
	}
	adrc.ESO_state[2] = ESO_state[2];
	adrc.yaw_out = yaw_out;
	adrc.target_rate = target_rate;
	xQueueOverwrite(queueADRC, &adrc);
}

/*
 * ADRC自抗扰控制器
 * */
void CONTROL_ALTITUDE::ADRC_Avcs(){
	static nonLinearController controllerChoice = Controller3;
	static float u0 = 0.0f;
	static float e = 0.0f;
//	static float e1 = 0.0f;
//	static float e2 = 0.0f;
	static float TD_state[2] = {0,0};
	static float ESO_state[3] = {0,0,0};
	static float h = CONTROL_DT;
	float fh;
	int coll_PWM_inc = 0;
	float coll_yaw_compensation_gain=0.0f;
	int yaw_offset = 0;

	int target_pwm = 0;
	float d_rate_error = 0.0f;
	float fe;
	float fe1;

	if(clear_flag == true){
		for(int idx = 0; idx < 2; ++idx){
			TD_state[idx] = 0.0f;
			ESO_state[idx] = 0.0f;
		}
		ESO_state[2] = 0.0f;
		yaw_out = 0.0f;
		clear_flag = false;
	}

	xQueuePeek(queueGyrDatFil, &gyro_filter, 0);
	xQueuePeek(queueESKF, &eskf_output, 0);
	xQueuePeek(queueADRC, &adrc, 0);

	target_pwm = auto_servo_width[3] - pwm_yaw_mid;
//	target_pwm = 0;

	if((control_mode == MODE_MANUAL) || (control_mode == MODE_ALTITUDE))
	{
		if(target_pwm > 30)//23
			target_pwm = target_pwm - 30;
		else if(target_pwm < -30)
			target_pwm = target_pwm + 30;
		else target_pwm = 0;
	}

	raw_target_rate = target_pwm * GAIN_REMOTE_TO_RATE;
	if(fabs(raw_target_rate)>=fabs(target_rate))
	{
		float e_tr = raw_target_rate  - target_rate;
		if(e_tr > RATE_DECREASEMENT)
			target_rate += RATE_DECREASEMENT;
		else if(e_tr < -RATE_DECREASEMENT)
			target_rate -= RATE_DECREASEMENT;
		else
			target_rate += e_tr;
	}
	else
	{
		float e_tr = raw_target_rate  - target_rate;
		if (e_tr > RATE_INCREASEMENT)
			target_rate += RATE_INCREASEMENT;
		else if(e_tr < -RATE_INCREASEMENT)
			target_rate -= RATE_INCREASEMENT;
		else
			target_rate += e_tr;
	}
	if (target_rate > LIMIT_RATE)
		target_rate = LIMIT_RATE;
	if (target_rate < -LIMIT_RATE)
		target_rate = -LIMIT_RATE;
	test_target_rate = target_rate;

	//低通滤波，没用上
	d_rate_error = (rate_error - _rate_error)*ALPHA;
	rate_error = d_rate_error + _rate_error;
	_rate_error = rate_error;

	coll_PWM_inc = pwm_coll_mid - auto_servo_width[5]; //总距PWM增加值，这是前馈通道补偿值，利用前馈通道使得尾翼快速跟踪总距的变化
	coll_yaw_compensation_gain = 0.1 * 0.00864;//out_loop_Y_PID[0] 0.42f pidVelX->Kp
	yaw_offset = coll_yaw_compensation_gain * coll_PWM_inc; //增益通过实验测出

	/*
	 *
	 * ADRC部分
	 * v:期望值，代表这里的target_rate
	 * v1:过渡状态，将v平滑化
	 * v2:v的微分，并考虑到了传统微分对于噪声的放大的影响
	 * z1:ESO观测到的v1的状态
	 * z2:ESO观测到的v2的状态
	 * z3:ESO可观的所有误差的状态
	 * u0:控制器输出状态
	 * u:考虑到z3补偿后的最终输出状态，进入下一级系统，也相当于经过PID环节后的输出
	 *
	 * */

	/*
	 * TD部分
	 * @args
	 * r:速度因子，当速度因子越小，v1曲线越平缓；速度因子越大，v1曲线变化越大，并无限接近于v
	 * h0：一般比h大，当h0较大时，能够明显减少震荡，但是太大又会造成v1的变化过于平缓
	 * */
	//args
	static float r = 50.0f; //2.0f
	static float h0 = 40 * h; // 5 * h

	fh = fhan(TD_state[0] - target_rate,TD_state[1], r, h0);
	TD_state[0] = TD_state[0] + h * TD_state[1];
	TD_state[1] = TD_state[1] + h * fh;

	/*
	 * ESO部分
	 * @args
	 * delta:一般取h的整数倍，1-5倍
	 * beta01:
	 * beta02：
	 * beta03:
	 * b0:dx2 / dt = f(x1, x2) + b0 * u，补偿因子
	 * */
	static float beta01 = 0.1f; //100x
	static float beta02 = 2.5f; //300
	static float beta03 = 5.0f; //1000
	static float delta = 5 * h; // h
	static float b0 = 50.0f; //补偿因子，过小会使u产生震荡，过大会抑制v1的     //过大！

	beta01 = pidAcc_z->Kp * 10, beta02 = pidAcc_z->Ki * 10, beta03 = pidAcc_z->Kd * 10;
//	delta = pidTest1->Kp * 10, b0 = pidTest2->Ki * 10;

	e = ESO_state[0] - (gyro_filter.gyro_filter[2] - eskf_output.Gyro_bias[2]);
//	e = ESO_state[0] - 0;
	fe = fal(e, 0.5, delta);
	fe1 = fal(e, 0.25, delta);
	ESO_state[0] = ESO_state[0] + h * (ESO_state[1] - beta01 * e);
	ESO_state[1] = ESO_state[1] + h * (ESO_state[2] - beta02 * fe + b0 * yaw_out);
	ESO_state[2] = ESO_state[2] + h * (-beta03 * fe1);

	/*
	 * 误差反馈率部分
	 * u0 = -fhan(e1,c * e2, r1, h1)
	 * @args
	 * c = 0.6-0.7
	 * h1 = k * h;
	 * r1 = 5
	 * beta1:对应比例误差e1 = v1 - z1的参数
	 * beta2:对应微分误差e2 = v2 - z2的参数
	 * */
	if(controllerChoice == Controller1){
//		float c = 0.05f; //阻尼因子0.6
//		float r1 = 20.0f; //20f
//		float h1 = 30 * h; // 30 * h
		float c = pidTest2->Kp * 10, r1 = pidTest2->Ki * 10, h1 = pidTest2->Kd * 10;
		u0 = nonLinearController1(TD_state, ESO_state, c, r1, h1);
	}else if(controllerChoice == Controller2){
		u0 = nonLinearController2(TD_state, ESO_state, h);
	}else{
		/*
		 * beta0 = 2.5 beta1 = 5 beta2 = 2.5
		 * theta0 = 0.25 theta1 = 0.75 theta2 = 1.5
		 * delta = 5 * h
		 * */
		float beta0 = pidTest2->Ki * 10, beta1 = pidTest2->Kp * 10, beta2 = pidTest2->Kd * 10; //beta0-->积分系数，beta1-->比例系数， beta2-->微分系数
		//float alpha0 = pidTest2->Kp * 10, alpha1 = pidTest2->Ki * 10, alpha2 = pidTest2->Kd * 10; // 0.25 0.75 1.5   -0.6 0.6 1.2
		float alpha0 = 0.0f, alpha1 = 0.0f, alpha2 = 0.0f;
		float delta = 5 * h;
		u0 = nonLinearController3(TD_state, ESO_state, h, beta0, beta1, beta2, alpha0, alpha1, alpha2, delta);
	}

	/*
	 * 输出
	 * */
//	yaw_out = u0 - ESO_state[2] / b0;
	yaw_out = u0;
	/*限幅输出控制量*/
	if(yaw_out > OUTPUT_LIMIT){
		yaw_out = OUTPUT_LIMIT;
	}
	else if(yaw_out < -OUTPUT_LIMIT){
		yaw_out = -OUTPUT_LIMIT;
	}

	auto_servo_width[3] = pwm_yaw_mid + yaw_out / GAIN_REMOTE_TO_RATE;

	for(int idx = 0; idx < 2; ++idx){
		adrc.TD_state[idx] = TD_state[idx];
		adrc.ESO_state[idx] = ESO_state[idx];
	}
	adrc.ESO_state[2] = ESO_state[2];
	adrc.target_rate = target_rate;
	adrc.yaw_out = yaw_out;

	xQueueOverwrite(queueADRC, &adrc);
}

void CONTROL_ALTITUDE::Heli_Attitude_control(void)
{
	Att_Loop_count = (Att_Loop_count+1)>50000? 1000 : Att_Loop_count+1;
	Pos_Loop_count = 0;
	loop_count = Att_Loop_count;

	if (Att_Loop_count == 1)
	{
		xQueuePeek(queueESKF, &eskf_output, 0);
		yaw_command = eskf_output.Attitude[2];
	}
	else if(auto_yaw_control_mode==0)
	{
		xQueuePeek(queueESKF, &eskf_output, 0);
		yaw_command = eskf_output.Attitude[2];
	}
	Flybarless_H1_DecoupingMatrix();
	Attitude_Control_Step();
	Flybarless_CoupingMatrix();
	Flybarless_servos_widths_output();
}

